#include <stdio.h>
#include <assert.h>
#include "tc.h"

static GLfloat getArea(int N, boid_t bs[])
{
	int i;
	GLfloat areaTimes2 = 0.0;

	for (i = 0; i < N - 1; i++) {
		areaTimes2 += ((bs[i].x * bs[i + 1].y) 
			       - (bs[i + 1].x * bs[i].y));
	}

	areaTimes2 += ((bs[N - 1].x * bs[0].y) 
		       - (bs[0].x * bs[N - 1].y));

#if defined(__TOC_DEBUG__)
	printf("DEBUG: Area(%3.3f)\n", areaTimes2 / 2);
#endif /* __TOC_DEBUG__ */

	return (areaTimes2 / 2);
}

TocPoint_t getCentroid(int N, boid_t bs[])
{
	int i;
	TocPoint_t centroid;
	GLfloat aggX = 0.0;
	GLfloat aggY = 0.0;

	if (N == 1) {
		centroid.x = bs[0].x;
		centroid.y = bs[0].y;
		return centroid;
	}

	GLfloat A = getArea(N, bs);
	/* assert(A != 0); */

	if (A == 0.0)
		A = 1.0;

	centroid.x = 0.0;
	centroid.y = 0.0;

	for (i = 0; i < N - 1; i++) {
		const GLfloat commonTerm  = (bs[i].x * bs[i + 1].y) 
			- (bs[i + 1].x * bs[i].y);
		aggX += (bs[i].x + bs[i + 1].x) * commonTerm;
		aggY += (bs[i].y + bs[i + 1].y) * commonTerm;
	}

	const GLfloat commonTerm  = (bs[N - 1].x * bs[0].y) 
		- (bs[0].x * bs[N - 1].y);
	aggX += (bs[N - 1].x + bs[0].x) * commonTerm;
	aggY += (bs[N - 1].y + bs[0].y) * commonTerm;

	centroid.x = aggX / (6 * A);
	centroid.y = aggY / (6 * A);

#if defined(__TOC_DEBUG__)
	printf("DEBUG: Centroid (%1.3f, %1.3f)\n", 
	       centroid.x, centroid.y);
#endif /* __TOC_DEBUG__ */

	return centroid;
}
